/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.commands.CommandBase;
import edu.wpi.first.wpilibj.templates.commands.ManualArmDrive;

/**
 *
 * @author Jarrod Risley
 */
public class ArmSubsystem extends PIDSubsystem {

    /*
     * Global Class Variables
     */
    private Victor leftArm;
    private Victor rightArm;
    private AnalogChannel stringPot;
    
    private static final double Kp = 0.05;
    private static final double Ki = 0.0;
    private static final double Kd = 0.050;
    
    private static final double MIN_IMPULSE_DURATION = 1.0;
    private static final double TIME_TO_TRAVERSE_COUNT = 0.007; 
    
    public static final double MIN_POT_COUNT = 61;
    public static final double MAX_POT_COUNT = 422;

    private String subsystemName;
    private double lastCommandTime;
    private double impulseDuration;
    private boolean motorsEnabled;
    
    private boolean useCamera = false;
    
    public static class PresetArmPosition {
        
        /*
         * Inner Class Variables
         */
        
        // These are in counts of the String Pot.
        private int value;
        private static final int onGround = 50; //ORIGINALLY: 65
        private static final int thirtyDegrees = 200;
        private static final int aboveThirty = 350;
        private static final int mailBox = 422;
        private static final int onBox = 555;
        
        public static final ArmSubsystem.PresetArmPosition ON_GROUND =
                new ArmSubsystem.PresetArmPosition(onGround);
        
        public static final ArmSubsystem.PresetArmPosition THRITY_DEGREES = 
                new ArmSubsystem.PresetArmPosition(thirtyDegrees);
        
        public static final ArmSubsystem.PresetArmPosition ABOVE_THIRTY =
                new ArmSubsystem.PresetArmPosition(aboveThirty);
        
        public static final ArmSubsystem.PresetArmPosition MAIL_BOX_POSITION =
                new ArmSubsystem.PresetArmPosition(mailBox);
        
        public static final ArmSubsystem.PresetArmPosition IN_BOX = 
                new ArmSubsystem.PresetArmPosition(onBox);
        
        
        // INNER CLASS CONSTRUCTOR //
        
        
        private PresetArmPosition(int value) {
            this.value = value;
        } // end PresetArmPosition Constructor
        
        
        // INNER CLASS METHOD //
        
        /**
         * Gets the value of the PresetArmPosition.
         * 
         * @return value - the value of the position, in pot counts.
         */
        public int getValue() {
            return value;
        } // end getValue
        
    } // end PresetArmPostition Inner Class
    
    
    // CONSTRUCTOR //
    
    /**
     * Creates an ArmSubsystem.
     * 
     * @param name The name of the subsystem.
     * @param sidecarNum The sidecar the subsystem is attached to.
     * @param pwmPort The port to which the victors are attached.
     * @param potPort The arm's string pot port.
     */
    public ArmSubsystem(String name, int sidecarNum, int leftPort, int rightPort, int potPort) {
        super("ArmSubsystem", Kp, Ki, Kd);

        System.out.println("Creating ArmSubsystem " + name + "...");
        subsystemName = name;
        
        // Use these to get going:
        // setSetpoint() -  Sets where the PID controller should move the system
        //                  to
        // enable() - Enables the PID controller.
        
        leftArm = new Victor(sidecarNum, leftPort);
        rightArm = new Victor(sidecarNum, rightPort);
        stringPot = new AnalogChannel(potPort);
        
        LiveWindow.run();
        LiveWindow.addActuator(subsystemName, "Left Arm Motor", leftArm);
        LiveWindow.addActuator(subsystemName, "Right Arm Motor", rightArm);
        LiveWindow.addSensor(subsystemName, "Pot", stringPot);
        LiveWindow.addActuator(subsystemName, "PID Controller", getPIDController());
        
        System.out.println("String pot reads: " + stringPot.getAverageValue());
        
    } // end ArmSubsystem Constructor
    
    
    // METHODS //
    
    /**
     * The default command for this subsystem is ManualArmDrive, which grants
     * the Co-Driver control of the arm via RIGHT THUMBSTICK.
     */
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
        setDefaultCommand(new ManualArmDrive());
    } // end initDefaultCommand
    
    /**
     * Returns the sensor value for the PID controller (that is invisible here).
     * @return The String Pot reading in terms of pot counts.
     */
    protected double returnPIDInput() {
        // Return your input value for the PID loop
        // e.g. a sensor, like a potentiometer:
        // yourPot.getAverageVoltage() / kYourMaxVoltage;
        
        System.out.println("String pot reads: " + stringPot.getAverageValue());
        
        if (!useCamera)
            return stringPot.getAverageValue();
        else
            return CommandBase.kittyNet.getYError();
        
    } // end returnPIDInput
    
    /**
     * The computed output value is applied to the motors...DON'T USE THIS 
     * METHOD - it works on its own since PIDSubsystem's PIDController uses 
     * this.
     * 
     * @param output - Not to be tampered with.
     */
    
    protected void usePIDOutput(double output) {
        // Use output to drive your system, like a motor
        // e.g. yourMotor.set(output);
        leftArm.set(-output);
        rightArm.set(-output);
        
//        if ( (motorsEnabled) && (Timer.getFPGATimestamp() > lastCommandTime 
//                + impulseDuration)) {
//            
//            motorsEnabled = false;
//            disable();
//            
//        } // end if
//        
//         setDefaultCommand(new ManualArmDrive());
        
    } // end usePIDOutput
    
    /**
     * Sets the position of the Arm to a defined position.
     * 
     * @param position - A PresetArmPosition.
     */
    
    public void setPosition(ArmSubsystem.PresetArmPosition position) {
        
//        enable(); // turns the PIDController on.
        impulseDuration = MIN_IMPULSE_DURATION + TIME_TO_TRAVERSE_COUNT * (Math.abs(position.getValue() - getLocation()));
        motorsEnabled = true;
        lastCommandTime = Timer.getFPGATimestamp();
        setSetpoint(position.getValue());
        
    } // end setPosition
    
    public void setPosition(double position) {
        
//        enable(); // turns the PIDController on.
        impulseDuration = MIN_IMPULSE_DURATION + TIME_TO_TRAVERSE_COUNT * (Math.abs(position - getLocation()));
        motorsEnabled = true;
        lastCommandTime = Timer.getFPGATimestamp();
        setSetpoint(position);
        
    } // end setPosition
    
    /**
     * Gets the current location of the subsystem.
     * 
     * @return returnPIDInput()
     */
    
    public int getLocation() {
        return stringPot.getAverageValue();
    } // end getLocation
    
    /**
     * Used for manually controlling the ArmSubsystem for minor adjustments
     * with a joystick value.
     * 
     * @param input - A joystick value.
     */
    
    public void manualOverride(double input) {
        
        leftArm.set(-input);
        rightArm.set(-input);
        
//        System.out.println(subsystemName + " pot reads: " + stringPot.getValue());
        
    } // end manualOverride
    
    /**
     * Allows for an autonomous command (namely Track) to input it's own values.
     * 
     * @param input Pot value to go to.
     */
    
    public void manualPIDOverride(double input) {
        enable();
        setSetpoint(input);
    } // end manualPIDOverride
    
    /**
     * Checks to see if the motors are enabled, meaning that another 
     * autonomous command is running.
     * 
     * @return True if the motors are running.
     */
    
    public boolean areMotorsEnabled() {
        return motorsEnabled;
    } // end areMotorsEnabled
    
    /**
     * Sets the arm to use the camera instead of the String Pot
     * @param setCameraUse 
     */
    public void setArmToCameraMode(boolean setCameraUse) {
        useCamera = setCameraUse;
    } // end setArmToCameraMode
    
    /**
     * Measures the rate of movement of the arm through the pot.
     * 
     * @return The speed of the arm's movement.
     */
    
    public double getRateOfArm() {
        
        double startTime = Timer.getFPGATimestamp();
        double potReadings[] = new double[10];
        
        for (int z = 0; z < potReadings.length; z++) {
            potReadings[z] = stringPot.getAverageValue();
        } // end for
        
        return (potReadings[9] - potReadings[0]) 
                / (Timer.getFPGATimestamp() - startTime);
        
    } // end getRateOfArm
    
} // end ArmSubsystem
